﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Graphics;

namespace FrogEngine
{
	public class FirstPersonCamera : BaseCamera
	{
		public FirstPersonCamera(IEngine engine, IOriented3DGameEntity entity, float mass)
			: base(engine, entity.Position, entity.World)
		{
			this.entityToFollow = entity;
			this.Position = entity.Position;
			this.Forward = entity.Forward;
			this.Right = entity.Right;
			this.Up = entity.Up;
			this.Mass = mass;
			this.NearPlane = 1;
			this.FarPlane = 100000000;
			this.AspectRatio = engine.GraphicsDevice.DisplayMode.AspectRatio;
			this.XSensitivity = 0.002f;

			Services.Add(engine.GetService<GamePadInputService>());
			Services.Add(engine.GetService<KeyboardInputService>());
			Services.Add(engine.GetService<MouseInputService>());

			foreach (var service in Services)
				service.Subscribe(this);

			this.wHeight = engine.GraphicsDevice.PresentationParameters.BackBufferHeight;
			this.wWidth = engine.GraphicsDevice.PresentationParameters.BackBufferWidth;
		}

		private IOriented3DGameEntity entityToFollow;
		private float yaw;
		private float pitch;
		private float roll;
		private float wWidth;
		private float wHeight;

		public float XSensitivity { get; set; }

		public override void Update(GameTime gameTime)
		{
			float dt = (float)gameTime.ElapsedGameTime.TotalMilliseconds;

			//this.Position = new Vector3(0, 5, 0);
			//this.Velocity = entityToFollow.Velocity;
			//this.Forward = entityToFollow.Forward;
			//this.Right = entityToFollow.Right;
			//this.Up = entityToFollow.Up;
			//this.World = entityToFollow.World;

			View = Matrix.CreateLookAt(Position, Position + Forward, Up);
			Projection = Matrix.CreatePerspectiveFieldOfView(Fov, AspectRatio, NearPlane, FarPlane);
		}

		private float yawSpeed = 0.1f;
		private float pitchSpeed = 0.1f;
		private float velocity = 10f; // units per millisecond
		public override void HandleInput(GameTime gameTime, PlayerIndex index, GamePadState currentState, GamePadState previousState)
		{
			float dt = (float)gameTime.ElapsedGameTime.TotalMilliseconds;
			float lx = currentState.ThumbSticks.Left.X;
			float ly = currentState.ThumbSticks.Left.Y;
			float rx = -currentState.ThumbSticks.Right.X;
			float ry = currentState.ThumbSticks.Right.Y;

			yaw = rx * yawSpeed * dt;
			pitch = ry * pitchSpeed * dt;

			Matrix yawMat = Matrix.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(yaw));
			Matrix pitchMat = Matrix.CreateFromAxisAngle(Right, MathHelper.ToRadians(pitch));
			Matrix rollMat = Matrix.CreateFromAxisAngle(Forward, MathHelper.ToRadians(roll));

			var mat = yawMat * pitchMat * rollMat;

			Forward = Vector3.Normalize(Vector3.Transform(Forward, mat));
			Right = Vector3.Normalize(Vector3.Transform(Right, mat));
			Up = Vector3.Normalize(Vector3.Transform(Up, mat));

			Right = new Vector3(Right.X, 0.0f, Right.Z);
			Up = Vector3.Cross(Right, Forward);

			Vector3 oldPos = this.Position;
			this.Position = this.Position +
				this.Forward * ly * velocity * dt +
				this.Right * lx * velocity * dt;
			this.Velocity = (Position - oldPos) / dt;
		}

		float xOrient = 0, yOrient = 0;
		public override void HandleInput(GameTime gameTime, MouseState currentState, MouseState prevState, float dx, float dy)
		{
			if (currentState.LeftButton == ButtonState.Pressed)
				this.engine.Exit();

			float yRatio, xRatio;

			xOrient -= dx * XSensitivity;
			xRatio = xOrient / (float)wWidth;

			yOrient += dy;
			if (yOrient >= wHeight)
			{
				yOrient = wHeight;
				yRatio = 1f;
			}
			else if (yOrient <= 0)
			{
				yOrient = 0;
				yRatio = 0;
			}
			else
				yRatio = (float)yOrient / (float)wHeight;

			float yaw = -MathHelper.Lerp(-MathHelper.PiOver2, MathHelper.PiOver2, xRatio);
			float pitch = -MathHelper.Lerp(-MathHelper.PiOver2, MathHelper.PiOver2, yRatio);

			Matrix yawMat = Matrix.CreateFromAxisAngle(Vector3.Up, xOrient);
			Matrix pitchMat = Matrix.CreateFromAxisAngle(Right, pitch);

			var mat = yawMat * pitchMat;

			Forward = Vector3.Normalize(Vector3.Transform(Vector3.Forward, mat));
			Right = Vector3.Normalize(Vector3.Transform(Vector3.Right, mat));
			Up = Vector3.Normalize(Vector3.Transform(Vector3.Up, mat));

			Right = new Vector3(Right.X, 0.0f, Right.Z);
			Up = Vector3.Cross(Right, Forward);
		}

		public override void HandleInput(GameTime gameTime, PlayerIndex index, KeyboardState currentState, KeyboardState previousState)
		{
			float dt = (float)gameTime.ElapsedGameTime.TotalMilliseconds;
			if (currentState.IsKeyDown(Keys.W))
				this.Position += this.Forward * velocity * dt;
			if (currentState.IsKeyDown(Keys.S))
				this.Position -= this.Forward * velocity * dt;
			if (currentState.IsKeyDown(Keys.D))
				this.Position += this.Right * velocity * dt;
			if (currentState.IsKeyDown(Keys.A))
				this.Position -= this.Right * velocity * dt;
		}
	}
}
